Approximate Inverse Kinematics using Jacobian

Jacky Baltes
National Taiwan Normal University
Taipei, Taiwan
jacky.baltes@ntnu.edu.tw

09 May 2022
# These libraries provide us with the most often used features

import matplotlib.pyplot as plt
import matplotlib.patches as mpatches
import numpy as np
import math
import random

import jb_geometry as jbg

Robotis Thormang Arms Kinematics

J_1(θ_1)
J_1(θ_1)
J_1(θ_2)
J_1(θ_2)
O_2
O_2
O_0
O_0
O_1
O_1
O_2
O_2
O_3
O_3
O_4
O_4
O_6
O_6
O_5
O_5
Viewer does not support full SVG 1.1
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )

fig5 = addJBFigure("fig5", 0, 0, fig )
from matplotlib.animation import ArtistAnimation

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], frame )
  #ax.view_init(elev=10, azim = i * 4 )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )

fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )

fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )

fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )

fig5 = addJBFigure("fig5", 0, 0, fig )
from matplotlib.animation import ArtistAnimation

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], frame )
  #ax.view_init(elev=10, azim = i * 4 )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )

fig5 = addJBFigure("fig5", 0, 0, fig )
from matplotlib.animation import ArtistAnimation

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi] )

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], frame )
  #ax.view_init(elev=10, azim = i * 4 )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, **, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )

fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, *, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax )

fig5 = addJBFigure("fig5", 0, 0, fig )
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, *, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0 ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = leftShoulderTranslation.dot( leftShoulderRotation )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j1A ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax=ax )

fig5 = addJBFigure("fig5", 0, 0, fig )

Thormang Forward Kinematics

Thormang Forward Kinematics

Thormang Forward Kinematics

Thormang Forward Kinematics

Thormang Forward Kinematics

Thormang Forward Kinematics

solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

print(n1)


print(pos_end_effector1[0:3])
[[-8.66025404e-01  4.53153894e-01  2.11309131e-01  6.37498350e-02]
 [-4.34682641e-17 -4.22618262e-01  9.06307787e-01  7.05649271e-01]
 [ 5.00000000e-01  7.84885567e-01  3.65998151e-01  5.10417953e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
[0.08488075 0.79628005 0.54701777]
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.extend( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()

Thormang Inverse Kinematics (Iterative)

solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
45
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  #frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  #frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  #frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  #frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
15
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( solution1, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
import jb_geometry as jbg

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

LEFT_SHOULDER_LATERAL, LEFT_SHOULDER_FRONTAL, LEFT_ELBOW_LATERAL = range(3)

def thormang_forward_kinematics( thetas, *, A = None, ax = None, frame_in = None ):
  frame = []
  # Draw the world coordinate system
  if A is None:
    A = np.eye(4)

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.5, 3.0, A ) ) 

  # Draw the left shoulder Joint 1 (Lateral)
  leftShoulderTranslation = jbg.translate( 0.0, 0.225, 0.40 )
  leftShoulderRotation = jbg.rotate_x( -90.0/180.0 * math.pi )
  j1A = A.dot( leftShoulderTranslation.dot( leftShoulderRotation ) )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, A.dot( j1A ) ) )

  j2_d = 0.1 # m
  j2_a = 0.05 # m
  j2T = jbg.DHH( ( 90.0/180.0*math.pi + thetas[LEFT_SHOULDER_LATERAL] ), j2_d, j2_a, 90.0/180.0 * math.pi )

  j2A = j1A.dot(j2T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j2A ) )

  #jbg.drawLink(ax, j1A, j2A, width=25 )

  j3_d = 0.2
  j3_a = 0.05

  # Converted into one rotation around Z and one translation along X
  j3_dp = 0.0
  j3_ap = math.hypot( j3_d, j3_a)
  j3_theta = 90.0/180.0 * math.pi - math.atan2( j3_a, j3_d )
  j3T = jbg.DHH( (j3_theta + thetas[LEFT_SHOULDER_FRONTAL]), j3_dp, j3_ap, 0.0/180.0 * math.pi )

  j3A = j2A.dot(j3T)
  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.25, 2.0, j3A ) )

  # Joint 4
  j4_d = 0.22
  j4_a = -0.05

  # Convert into one translation along the X axis
  j4_dp = 0.0
  j4_ap = math.hypot( j4_d, j4_a)
  j4_theta = math.atan2( j3_a, j3_d ) + math.atan2( -j4_a, j4_d )

  #Breaking Denavit Hartenberg Convention
  j4Td = jbg.DHH( j4_theta, j4_dp, j4_ap, 0.0/180.0 * math.pi )
  j4T = jbg.DHH( j4_theta + thetas[LEFT_ELBOW_LATERAL], j4_dp, j4_ap, 0.0/180.0 * math.pi ).dot(jbg.rotate_y(90.0/180.0*math.pi)).dot(jbg.rotate_x( math.atan2( -j4_a, j4_d ) ) )

  j4A = j3A.dot(j4T)
  j4Ad = j3A.dot( j4Td )

  if ax is not None:
    frame.extend( jbg.plot_coordinate_system( ax, 0.15, 2.0, j4A ) )
  #jbg.plot_coordinate_system( ax, 0.15, 2.0, j4Ad )
  if ( frame_in is not None ):
    frame_in.extend( frame )
  return j4A

a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0/180.0*math.pi, 45.0/180.0*math.pi], ax=ax )

fig5 = addJBFigure("fig5", 0, 0, fig )

Thormang Forward Kinematics

from matplotlib.animation import ArtistAnimation

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  a_03 = thormang_forward_kinematics( [20.0/180.0*math.pi, -20.0+i*2/180.0*math.pi, 45.0/180.0*math.pi], ax=ax, frame_in=frame )
  #ax.view_init(elev=10, azim = i * 4 )
  #print('frame', frame )
  frames.append( frame.copy() )

#print('frames', frames )
ani = ArtistAnimation( fig, frames, interval=100)

a1 = addJBAnimation("a1", 0, 0, ani )
plt.close()

Thormang Forward Kinematics

solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

print(n1)


print(pos_end_effector1[0:3])
[[-8.66025404e-01  4.53153894e-01  2.11309131e-01  6.37498350e-02]
 [-4.34682641e-17 -4.22618262e-01  9.06307787e-01  7.05649271e-01]
 [ 5.00000000e-01  7.84885567e-01  3.65998151e-01  5.10417953e-01]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
[0.08488075 0.79628005 0.54701777]
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = rotate_z( i * 4/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = rotateZ( i * 4/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0.1,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = jbg.rotate_z( i * 4/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [target[0]], [target[1]], [target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17

Thormang Inverse Kinematics (Iterative)

solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = jbg.rotate_z( i * 4/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

print(len(frames[0]))
ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()
17
solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = jbg.rotate_z( i * 4/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector1[0], target[0]], [ pos_end_effector1[1], target[1]], [ pos_end_effector1[2], target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()

Thormang Inverse Kinematics (Iterative)

solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = jbg.rotate_z( -i * 2/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector1[0], a_target[0]], [ pos_end_effector1[1], a_target[1]], [ pos_end_effector1[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()

Thormang Inverse Kinematics (Iterative)

solution1 = [30.0/180.0*math.pi, 25.0/180.0*math.pi, 80.0/180.0*math.pi ]
n1 = thormang_forward_kinematics( solution1 )
pos_end_effector1 = n1.dot([0,0,0,1])

solution2 = [15.0/180.0*math.pi, -25.0/180.0*math.pi, 120.0*math.pi ]
n2 = thormang_forward_kinematics( solution2 )
target = n2.dot([0,0,0.1,1])


fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range(45):
  frame = []
  A = jbg.rotate_z( -i * 2/180.0 * math.pi )
  a_03 = thormang_forward_kinematics( solution1, A = A, ax = ax, frame_in = frame )
  pos_end_effector = a_03.dot( [ 0, 0, 0.1, 1 ] )
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a2 = addJBAnimation("a2", 0, 0, ani )
plt.close()

Thormang Inverse Kinematics (Iterative)

Iterative Solution to Inverse Kinematics

Assume: complex robot system (e.g., humanoid from foot to arm, snake like robot) that we cannot break down into subchains that we can reduce to a two-link problem

Then: We can establish an error criteria (Euclidean distance of end effector to target location)

Iteratively approach the target location by moving closer toward the target in small steps

If current position of end effector is already close to the target location

Direction of Movement in each Iteration

Current configuration: θ_1 .. θ_n

Current end effector position: c_x, c_y, c_z. Solve forward kinematics problem

Rigid robots can easily solve the forward kinematics problem using DHH convention. Much harder for soft robots

How do we change the current configuration (joint angles) to move closer to the target?

Iterative Inverse Kinematics

Approch is to reduce the error. Multivariate optimization problem - Define error function (e.g., Euclidan distance to goal) and then minimize the error function

Minimize the error function -> calculate the derivative of the error function

Some applications (e.g., linear regression), we can calculate the derivative of the error function analytically. In more complex cases, we can calculate the derivative of the error function numerically. In this case, we find an approximate solution

Calculating the analytical solution may be too time consuming (inverting a large matix). Sometimes, we use an approximation instead of an analytical solution. This is especially true for problems with many dimensions (hundreds or thousands). Matrix inversion is O(n**3)

Approximation Algorithm

Move each joint individually and then check how the error changes

Numerically approximate the first (partial) derivative of the multivariate error function with respect to the current configuration θ_1 .. θ_n

Gradient of the error function

\[ \nabla e(\theta_1, \theta_2, \cdots, \theta_n ) = [ \frac{\delta e}{\delta \theta_1}, \frac{\delta e}{\delta \theta_2}, \cdots, \frac{\delta e}{\delta \theta_n} ] \]

Gradient descent algorithm - take a small step in the direction opposite of the gradient (minimize error)

\[ \theta_1 = \theta_1 - \alpha \frac{\delta e}{\delta \theta_1}, \theta_2 = \theta_2 - \alpha \frac{\delta e}{\delta \theta_2}, \cdots, \theta_n = \theta_n - \alpha \frac{\delta e}{\delta \theta_n} \] , where \( \alpha \) is a small constant \( ( 0 < \alpha \leq 1 ) \)

Gradient Descent Algorithm

Gradient descent algorithm can be written in matrix form

\[ \theta^{t+1} = \theta^{t} - \alpha \nabla e(\theta) \]

Jacobian

In inverse kinematics, the error consists of 3 components \( ( \Delta x, \Delta y, \Delta z ) \)

current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

def fwd_kinematics( thetas ):
  print("fwd_kinematics")
  e = thormang_forward_kinematics(thetas).dot([0.0, 0.0, 0.1, 1] )
  return e 

def Jacobian( thetas, fkin ):
  J = np.zeros( (3, len(thetas) ) )
  current_pos = fkin( thetas )
  dt = 1.0/180.0*math.pi
  
  for i,t in enumerate(thetas):
    thetas_d = thetas.copy()
    thetas_d[i] = thetas_d[i] + dt
    p = fkin( thetas_d )
    # print('t_d', thetas_d )
    # print('p', p)
    # print('J', J)
    J[0:3,i] = ( p[0:3] - current_pos[0:3] ) / dt
  return J


jac = Jacobian( current_angles, fwd_kinematics )
print("Current Angles", current_angles)
print("Current Position", current_position)

print('Jacobian\n', jac)
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
Current Angles [0.5235987755982988, 0.4363323129985824, 1.3962634015954636]
Current Position [0.14268179 0.39627408 0.64713211 1.        ]
Jacobian
 [[ 0.24587446  0.03417197 -0.06684805]
 [ 0.         -0.33596852 -0.29499608]
 [-0.14483112  0.05918759 -0.11578422]]

The Jaobian

The Jaobian provides us with the following information

Jacobian at position: ['30.00 deg.', '25.00 deg.', '80.00 deg.']

θ_1 θ_2 θ_3
X 0.25 0.03 -0.07
Y 0.00 -0.34 -0.29
Z -0.14 0.06 -0.12

Jacobian Solution

The following matrix equation relates \( \delta err \) with \( \delta \theta \)

\[ \delta err = J \delta \theta \]

Solve the equation by calculating the inverse of the Jacobian

\[ \delta \theta = J^{-1} \delta err \]

The Jacobian is usually not square (3 rows, # joints columns) and even if square, it may be singular

Apply the Moore Penrose Pseudo Inverse method (pre-multiply by the transpose of the Jacobian) \( J^{T} \)

\[ J^T \delta err = J^T J \delta \theta \\ (J^T J)^{-1} J^T \delta err = (J^T J)^{-1} (J^T J) \delta \theta \\ (J^T J)^{-1} J^T \delta err = \delta \theta \]
# Pseudo Inverse method

current_angles = np.array( solution1 )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

d_err = (target - current_position)[0:3]

alpha = 0.3

count = 0
errs = []

while( np.linalg.norm(d_err) > 0.1 ):
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  d_err = (target - current_position)[0:3]
  errs.append( np.linalg.norm(d_err) )

  print("d_err", np.linalg.norm(d_err), "target", target, "current_position", current_position )
  jac = Jacobian(current_angles, fwd_kinematics)

  #print("Jacobian", jac )

  d_theta = np.linalg.inv( jac.T.dot(jac) ).dot( jac.T).dot(d_err) 
  #print("d_theta", d_theta )
  #print("current_angles.shape", current_angles.shape)

  current_angles = current_angles + alpha * d_theta
  #print("current_angles.shape", current_angles.shape)
  
  #print("current_angles", current_angles )

  count = count + 1
  if (count > 40 ):
    break

fig = plt.figure()
ax = fig.add_subplot(1,1,1)

ax.plot(errs, "b-", linewidth=2)

e1 = addJBFigure("e1", 0, 0, fig )
plt.close()
d_err 0.6803841190440875 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.14268179 0.39627408 0.64713211 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.539377462685629 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.06912383 0.42159308 0.50169066 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.42302129012094863 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.00150497 0.47190383 0.4014226  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.40001725789713116 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.04978688  0.55773932  0.4599157   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.33144358985326183 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.02397445 0.61317654 0.39929704 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.30046636828614787 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.00445318 0.66487258 0.39922975 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.28110233673490215 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.0035492   0.7025137   0.39601305  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.20232447004242446 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.01655725  0.72182565  0.3198601   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.14136374442265123 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.03648416  0.73723855  0.26347302  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.10038365163024875 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.04667577  0.75028648  0.22560372  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.07246167693708219 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.0534846   0.7604632   0.20026643  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

def fwd_kinematics( thetas, *, A = None, ax = None, frame_in = None ):
  print("fwd_kinematics")
  e = thormang_forward_kinematics(thetas, A=A, ,ax=ax, frame_in=frame_in).dot([0.0, 0.0, 0.1, 1] )
  return e 

def Jacobian( thetas, fkin ):
  J = np.zeros( (3, len(thetas) ) )
  current_pos = fkin( thetas )
  dt = 1.0/180.0*math.pi
  
  for i,t in enumerate(thetas):
    thetas_d = thetas.copy()
    thetas_d[i] = thetas_d[i] + dt
    p = fkin( thetas_d )
    # print('t_d', thetas_d )
    # print('p', p)
    # print('J', J)
    J[0:3,i] = ( p[0:3] - current_pos[0:3] ) / dt
  return J


jac = Jacobian( current_angles, fwd_kinematics )
print("Current Angles", current_angles)
print("Current Position", current_position)

print('Jacobian\n', jac)
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

def fwd_kinematics( thetas, *, A = None, ax = None, frame_in = None ):
  print("fwd_kinematics")
  e = thormang_forward_kinematics(thetas, A=A,ax=ax, frame_in=frame_in).dot([0.0, 0.0, 0.1, 1] )
  return e 

def Jacobian( thetas, fkin ):
  J = np.zeros( (3, len(thetas) ) )
  current_pos = fkin( thetas )
  dt = 1.0/180.0*math.pi
  
  for i,t in enumerate(thetas):
    thetas_d = thetas.copy()
    thetas_d[i] = thetas_d[i] + dt
    p = fkin( thetas_d )
    # print('t_d', thetas_d )
    # print('p', p)
    # print('J', J)
    J[0:3,i] = ( p[0:3] - current_pos[0:3] ) / dt
  return J


jac = Jacobian( current_angles, fwd_kinematics )
print("Current Angles", current_angles)
print("Current Position", current_position)

print('Jacobian\n', jac)
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
Current Angles [0.5235987755982988, 0.4363323129985824, 1.3962634015954636]
Current Position [0.14268179 0.39627408 0.64713211 1.        ]
Jacobian
 [[ 0.24587446  0.03417197 -0.06684805]
 [ 0.         -0.33596852 -0.29499608]
 [-0.14483112  0.05918759 -0.11578422]]

The Jaobian

The Jaobian provides us with the following information

Jacobian at position: ['30.00 deg.', '25.00 deg.', '80.00 deg.']

θ_1 θ_2 θ_3
X 0.25 0.03 -0.07
Y 0.00 -0.34 -0.29
Z -0.14 0.06 -0.12

Jacobian Solution

The following matrix equation relates \( \delta err \) with \( \delta \theta \)

\[ \delta err = J \delta \theta \]

Solve the equation by calculating the inverse of the Jacobian

\[ \delta \theta = J^{-1} \delta err \]

The Jacobian is usually not square (3 rows, # joints columns) and even if square, it may be singular

Apply the Moore Penrose Pseudo Inverse method (pre-multiply by the transpose of the Jacobian) \( J^{T} \)

\[ J^T \delta err = J^T J \delta \theta \\ (J^T J)^{-1} J^T \delta err = (J^T J)^{-1} (J^T J) \delta \theta \\ (J^T J)^{-1} J^T \delta err = \delta \theta \]
# Pseudo Inverse method

current_angles = np.array( solution1 )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

d_err = (target - current_position)[0:3]

alpha = 0.3

count = 0
errs = []

while( np.linalg.norm(d_err) > 0.1 ):
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  d_err = (target - current_position)[0:3]
  errs.append( np.linalg.norm(d_err) )

  print("d_err", np.linalg.norm(d_err), "target", target, "current_position", current_position )
  jac = Jacobian(current_angles, fwd_kinematics)

  #print("Jacobian", jac )

  d_theta = np.linalg.inv( jac.T.dot(jac) ).dot( jac.T).dot(d_err) 
  #print("d_theta", d_theta )
  #print("current_angles.shape", current_angles.shape)

  current_angles = current_angles + alpha * d_theta
  #print("current_angles.shape", current_angles.shape)
  
  #print("current_angles", current_angles )

  count = count + 1
  if (count > 40 ):
    break

fig = plt.figure()
ax = fig.add_subplot(1,1,1)

ax.plot(errs, "b-", linewidth=2)

e1 = addJBFigure("e1", 0, 0, fig )
plt.close()
d_err 0.6803841190440875 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.14268179 0.39627408 0.64713211 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.539377462685629 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.06912383 0.42159308 0.50169066 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.42302129012094863 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.00150497 0.47190383 0.4014226  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.40001725789713116 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.04978688  0.55773932  0.4599157   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.33144358985326183 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.02397445 0.61317654 0.39929704 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.30046636828614787 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.00445318 0.66487258 0.39922975 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.28110233673490215 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.0035492   0.7025137   0.39601305  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.20232447004242446 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.01655725  0.72182565  0.3198601   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.14136374442265123 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.03648416  0.73723855  0.26347302  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.10038365163024875 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.04667577  0.75028648  0.22560372  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.07246167693708219 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.0534846   0.7604632   0.20026643  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  frames.append( frame )

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye()
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target )
  frame.extend( ax.plot([ pos_end_effector[0], a_target[0]], [ pos_end_effector[1], a_target[1]], [ pos_end_effector[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.2, 0.6, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.2, 0.6, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.2, 0.6, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6171148590704281
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5602591787176366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5079162799036441
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45915126894154806
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41433425819284286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37500733211206727
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34669374072592596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33413184288406866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3228893421701253
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32024359057275675
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3080615201700689
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2871093154347471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25888571704116103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23286732447681371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20955015449909548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18861526520041552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16980974306606894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15290904460171195
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13771347248722277
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12404552162852442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11174731299726161
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10067824175841517
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09071289949610226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08173926785436081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07365715566055361
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06637684571359614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05981791933024497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053908231084458365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.048583010763466626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043784073636250975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03945912350771325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03556113575491366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03204780971521377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02888108153014193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026026689938875004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02345378863816573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0211345997409344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019044103618979052
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017159761040851578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015461264040069292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.013930312391442566
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012550412949812002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.01130669942781996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010185770466930479
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009175544096260388
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.2, 0.6, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.29380026601450127
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2651871485914366
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23924422542888069
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21573752277975952
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1944609816267056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17522452717642426
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15784944827785663
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14216774335742954
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12802276222389447
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1152697469940783
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10377586903387712
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09341983174914892
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08409122145398475
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07568975198217288
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06812448701855986
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.061313080035714146
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05518104843775663
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04966108838932524
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0446924329144104
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04022025427966999
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03619511077062393
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.032572437214964056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.029312077933743894
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02637786024678644
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02373720625493644
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.021360780371437505
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.019222169958699596
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.017297596416621007
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.015565654135138045
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.014007074841196917
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.012604515018572486
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.011342364242818748
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.010206572442557989
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.009184494265207057
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.47759521031616575
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4297922860394924
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.38661204494647383
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34782131180937714
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3130786871311263
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.28200570860377633
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2542357020161144
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2294402051745769
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20734512016444775
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18775400813263857
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17062154773372695
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1564142584972816
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15042105988219276
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15940560262623146
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1468416332953813
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14425917913453273
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14573413327683793
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13934308311030083
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2897045377393598
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2618440410687554
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23682876110989673
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21438951325885722
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1943046548089601
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17641001321114638
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16063780860704832
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14717048060437218
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13739314193112528
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16563228391594728
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1512842145566422
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1397963283070927
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1380179146518387
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14520090393795487
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13620627709919664
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21087328495353225
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19116211287184248
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1736177148068067
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15819129736570625
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14512674028250427
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13623018839863663
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21982574756636436
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19915567643339913
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18071205121759965
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16438992677671183
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15026401890040897
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13909937368961442
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.25

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4045671431083639
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3044800338620111
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23199465654408294
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1805779541070138
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14688217196056932
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16621926269202328
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13893305529660363
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23980013468427452
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1878105266587548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15122064884507389
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1368537500252748
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3887675630186442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2987770337895822
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23160915379081484
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1819885130207531
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14767814141232105
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14200038582642377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16199139513131555
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1371430650350476
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32417123824186317
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25050691774508743
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19586404865348173
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1567320730362408
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1365439124879922
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3973536165494696
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3014451484414789
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23226617755186307
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18203652355112312
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.148352595528219
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15996330923600863
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13710577655159734
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 1.0965669190792435
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5295248642785918
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4184428152189937
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3183211437780796
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24542643707741113
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19182731590375865
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15419584951401089
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14063014975787155
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24950825072234772
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19513921420949518
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1562538621177821
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1366317908331659
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.8188631991270038
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6717984777136295
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.15

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45214968620079027
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3842613471515385
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3265872201058371
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2781762996938153
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23769453064475077
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20395603409518553
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.176178744953095
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1545634334643768
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14767999380189542
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15578592195129026
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14040328340960423
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22664498901025656
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1954826622084345
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16955540555622398
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14882410114610292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13625819893388044
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.625523468787739
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.8075179888332358
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.706442004259637
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6148707931635683
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5249174525168404
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.44656223279285495
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3812769027974004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3262820665889081
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2797556797428292
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2403727810314921
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.207121399065079
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17928180013299236
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15660001141692387
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14057598090916168
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17535428251470733
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.153187156513759
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1378085246843571
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17968241892255693
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15676903169390238
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13983258074594057
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14679834706269285
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13572489734760987
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.9314789526738959
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7811956234532861
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6646960953288696
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5768406512407355
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22230268373351464
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19111798899847962
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17261286678220175
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.05

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5036770607261558
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4784987547429801
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45452636379039946
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.43172380964568985
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4100512631712042
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3894656636781167
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3699217602660103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3513732333095507
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33377368473874014
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.31707742533964123
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3012400591208827
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2862188967457927
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.271973241473044
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25846459371905206
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2456568217296666
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23351635210382293
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22201245325659655
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.211117733926127
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20080909935068775
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19106972097208388
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18189348615537768
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17329652708465293
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16535427670130884
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15837528343887283
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15490661789744425
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1589255053583814
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1522894661113319
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1486619790130483
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17394814704284114
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1657355634896333
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.158059876160979
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15100219793901531
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14486366424913438
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14169901136281962
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18997582658182416
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18085405903592916
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17223431333049435
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1641115749114096
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15649732837144031
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14944142160273635
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14311154657675368
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.138272436018892
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.152134173588734
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14543329086090556
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13966599871035118
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.01

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5247972771326571
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5195520978331117
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5143583445697565
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5092155597236728
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5041232897285112
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4990810847742677
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.49408849854769343
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.489145088007263
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4842504131904142
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.47940403705062984
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4746055253218561
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.46985444640773477
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.46515037129315867
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4604928734757308
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4558815289148077
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.45131591599594056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4467956155086629
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4423202106357309
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.43788928695207996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4335024324319164
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.42915923746252466
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.42485929486350926
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4206021999103471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4163875503612478
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4122149464864472
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4080839910991754
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.40399428958764044
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.39994544994746617
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3959370828141019
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.39196880149480506
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3880402219998563
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3841509630727313
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.38030064621900384
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.376488895733802
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37271533872767587
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3689796051507762
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3652813278152647
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.36162014241591345
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.35799568754886457
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.35440760472854627
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.35085553840275235
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34733913596591626
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.343858047770609
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34041192713731333
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3370004303625262
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.02

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.519504920615106
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5091206439832692
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4989382319361884
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.48895444228275897
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4791660592848198
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4695698838935475
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4601627275863288
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4509414091413999
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.44190275371293225
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4330435936297849
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4243607704205687
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41585113765326953
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.40751156426063134
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.39933893809747156
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3913301695405428
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3834821949948331
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3757919802128858
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3682565233670602
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3608728578400887
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.35363805471830206
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34654922498578605
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3396035214276905
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33279814025782495
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3261303224903369
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.31959735507827186
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.31319657184364497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3069253542246662
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3007811318663015
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2947613830806446
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.28886363520385056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2830854648768519
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2774244982779464
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.27187841133683255
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.26644492996206426
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.261121830317514
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2559069391887739
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2507981344880814
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2457933459572723
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24089055614372873
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2360878017462591
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23138317545924994
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2267748284887365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22226097398029704
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2178398916970414
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21350993443474944
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.02

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 70 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.519504920615106
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5091206439832692
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4989382319361884
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.48895444228275897
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4791660592848198
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4695698838935475
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4601627275863288
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4509414091413999
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.44190275371293225
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4330435936297849
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4243607704205687
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41585113765326953
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.40751156426063134
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.39933893809747156
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3913301695405428
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3834821949948331
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3757919802128858
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3682565233670602
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3608728578400887
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.35363805471830206
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34654922498578605
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3396035214276905
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33279814025782495
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3261303224903369
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.31959735507827186
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.31319657184364497
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3069253542246662
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3007811318663015
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2947613830806446
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.28886363520385056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2830854648768519
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2774244982779464
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.27187841133683255
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.26644492996206426
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.261121830317514
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2559069391887739
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2507981344880814
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2457933459572723
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24089055614372873
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2360878017462591
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23138317545924994
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2267748284887365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22226097398029704
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2178398916970414
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21350993443474944
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20926953689101702
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20511722806879995
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20105164888569899
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19707157765117858
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19317596779513108
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1893640053459877
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18563519956827093
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18198953203948717
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17842771494676354
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17495166889902175
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17156548471410576
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16827759109196244
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16510648539706704
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16210009142144216
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15943719652525404
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15919144511017574
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15692352108230762
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20998251904396073
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2058220077132482
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20174686039138093
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1977555563542425
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19384664580394073
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19001875843605295
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18627061533331385
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18260104566915852
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.7, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.035

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 70 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5115797567573875
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4936826656827211
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.47638870064014643
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4596833451190141
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.44355172320194075
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4279785414768303
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4129481314485645
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3984445474239693
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3844516876436552
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3709534180524315
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3579336869677975
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3453766249705965
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3332666281686309
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.32158842523525705
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3103271298500571
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.29946828076440934
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.288997871951686
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.27890237536357365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2691687588321664
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25978450173988377
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2507376113367347
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24201664316450733
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.233610730188172
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22550962736756405
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21770378236717813
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21018445063196414
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20294388794991705
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19597568478983124
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18927537685531545
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18284163949884896
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17667885550035048
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17080341883273875
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16526258740161812
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16021266069481593
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15656458617694272
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.8933729501771535
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.861621500391537
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.8311528964286548
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.8019194419280213
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7739602977589151
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7477676317685317
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7298368824741681
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7156529391693222
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7037798954810436
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6990920415574864
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.7117514305252326
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6982809011838838
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6762690296810232
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6537218107662768
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6310009496814344
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.6118536605428496
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5903858432049177
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5697099029927063
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5497949162033311
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5306026640393319
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.5120998645668792
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4942575021284819
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4770493686334467
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4604511823494841
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.44444010901855174
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.42899449245969323
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41409369132533846
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.39971797153760397
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.38584842957200016
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3724669339231653
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.35955607799723827
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3470991406583499
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33508005222962056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3234833646182771
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.31229422474001656
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.3, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.1

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 70 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.400914290636055
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.36067559945073174
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3241355641870142
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2911484438727125
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2614993604404171
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23491635534686311
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21110579893057746
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18978029298768714
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17067333442429072
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15354455042623325
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13817994048795657
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12438996760320369
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11200696301728197
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1008825018607927
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09088500374112778
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08189762991503721
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07381647398796652
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06654901766978996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.060012819307999926
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05413440884026634
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04884837485354633
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0440966499347214
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.039828040544340915
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03599814150199943
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.032570037108809444
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02951707438208652
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026832823396395294
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.024578084768723824
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02337164083274722
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03033932633163899
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.027530829586948597
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.025056158156155587
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.022951598232604453
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.021504800555920592
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043779282974366056
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.039597670144300226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03583549280049822
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03245537920004466
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.029426068241693748
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02672436266839663
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.024341375839484987
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.022306606089976092
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.020838835714644746
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0259752275868011
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.023686172437226448
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.021767002433611463
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.020599479722649996
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2218631396787895
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20015160018286163
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18058825724385077
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16295603349281165
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.147061302060217
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1327307922832322
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11980914624518536
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10815688971030091
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09764869962660315
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08817190105340482
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07962515086731403
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07191727893712889
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06496626568555124
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05869834084761614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.053047193373354756
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.047953288167947146
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043363293672519974
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03922963912574416
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03551025131738925
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.032168594147856486
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.029174332928789203
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02650557296127787
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.024156075080956154
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.3, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.05

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 45 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4228306773278025
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4016975622537616
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3815563486605925
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.36237305449101315
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34411518590633167
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3267501789166017
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3102446403100308
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.29456419930768035
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2796737408647584
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.26553782012588817
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25212111617682537
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23938884243918865
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22730707661107863
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21584300218927668
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20496506920628105
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19464308826533264
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18484827310205434
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1755532454390088
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1667320134645302
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15835993273001614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.150413656014022
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1428710768751508
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13571127020608792
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1289144320510193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12246182018655694
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11633569642452475
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11051927121672071
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10499665088226866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09975278760353573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09477343222181248
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09004508979102574
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08555497780384821
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08129098698072033
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0772416445023826
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07339607956643347
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0697439911556715
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06627561791925649
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06298171008684846
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05985350336177115
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.056882694774280866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05406142052454169
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05138223591457777
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04883809757245078
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.046422348333322705
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.044128705402599666
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.3, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.05

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 70 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4228306773278025
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4016975622537616
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3815563486605925
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.36237305449101315
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34411518590633167
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3267501789166017
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3102446403100308
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.29456419930768035
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2796737408647584
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.26553782012588817
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25212111617682537
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23938884243918865
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22730707661107863
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21584300218927668
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20496506920628105
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19464308826533264
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18484827310205434
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1755532454390088
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1667320134645302
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15835993273001614
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.150413656014022
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1428710768751508
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13571127020608792
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1289144320510193
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12246182018655694
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11633569642452475
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11051927121672071
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10499665088226866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09975278760353573
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09477343222181248
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09004508979102574
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08555497780384821
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08129098698072033
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0772416445023826
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07339607956643347
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0697439911556715
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06627561791925649
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06298171008684846
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05985350336177115
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.056882694774280866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05406142052454169
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05138223591457777
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04883809757245078
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.046422348333322705
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.044128705402599666
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04195125286385002
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.039884440360004846
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03792309117923309
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03606242568250453
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.034298111550260184
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.032626364514104444
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.031044152520109714
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0295496349036038
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.028143212706960512
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0268304994417576
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.025633387203730103
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.024660285732514314
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.027363978067538004
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026079202575769355
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02488657033726163
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.023816114547724834
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.023030696301796
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04772047336037443
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.045379396804967304
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0431553547731283
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04104266534045308
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03903596077951482
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03713018111985442
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03532057208529972
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.033602689413755334
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.3, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.02

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 70 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4361087195011584
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.42739520092682864
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41884994519552954
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.41046998414833125
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4022524411746021
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3941945179288535
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3862934823730568
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.37854665826195305
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3709514161269173
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3635051657557301
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3562053501149881
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3490494406211251
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34203493363611576
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.33515934804472747
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3284202237607471
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3218151210084693
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3153416202312063
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.30899732248889483
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3027798502204286
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.29668684826169106
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.29071598502623974
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.28486495377131915
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2791314738866807
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.27351329215717907
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.26800818396202947
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.26261395438389135
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.25732843920957305
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2521495058112607
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24707505390286155
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.24210301617050736
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23723135877964652
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23245808176362678
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22778121930040487
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22319883988514164
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21870904640708583
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21430997613942465
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20999980065076584
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20577672564669733
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20163899074951125
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19758486922371044
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19361266765439591
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18972072558508088
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18590741512091302
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1821711405027336
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17851033765686908
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17492347372503697
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17140904657827835
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16796558431838432
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16459164476987787
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16128581496524363
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15804671062576167
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1548729756400006
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15176328154174992
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14871632698893483
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1457308372448345
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14280556366273803
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1399392831750003
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1371307977873127
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1343789340788678
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1316825427089883
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12904049793068428
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1264516971115168
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12391506026207043
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1214295295722653
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11899406895569166
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11660766360208893
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11426931953805365
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11197806319602722
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10973294099157374
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10753301890894114
current_angles = solution1
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])
target_position =  [ 0.5, 0.3, 0.4, 1 ]

# while( math.hypot( current_position - target ) > eps ):
#   # move current_angles
#   current_angles = inverse_kinematics_step(current_angles)
#   current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

# Jacobian is the matrix of partial derivatives: influence of theta_1 on x,y,z of end effector,
# influence of theta_2 on x,y,z of end effector (assuming all other angles remain constant)

def inverse_kinematics_step( thetas, alpha, target, fkin ):
  current_position = fkin( thetas )
  d_err = (target - current_position)[0:3]
  J = Jacobian( thetas, fkin )
  d_theta = np.linalg.inv( J.T.dot(J) ).dot( J.T).dot(d_err) 
  thetas = thetas + alpha * d_theta
  return thetas

eps = 0.01
alpha = 0.04

fig = plt.figure( figsize=(10,10) )
ax = fig.add_subplot(111, projection='3d')
ax.set_xlim((0,1))
ax.set_ylim((0,1))
ax.set_zlim((0,1))

frames = []
for i in range( 70 ):
  frame = []
  A = np.eye(4)
  a_03 = thormang_forward_kinematics( current_angles, ax = ax, frame_in = frame )
  current_angles = inverse_kinematics_step( current_angles, alpha, target_position, fwd_kinematics )
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  a_target = A.dot( target_position )
  frame.extend( ax.plot([ current_position[0], a_target[0]], [ current_position[1], a_target[1]], [ current_position[2], a_target[2]], ':', color=(0, 1, 1, 0.7), linewidth=3 ) )
  frame.append( ax.scatter( [a_target[0]], [a_target[1]], [a_target[2]], s=40, color=(0, 1, 1, 0.7 ) ) )
  frame.append( ax.scatter( [current_position[0]], [current_position[1]], [current_position[2]], s=40, color=(1, 1, 0, 0.7 ) ) )

  frames.append( frame )

  d_err = (target_position - current_position)[0:3]
  print('err', np.linalg.norm( d_err ) )
  if np.linalg.norm( d_err ) < eps:
    break

ani = ArtistAnimation( fig, frames, interval=100)

a3 = addJBAnimation("a3", 0, 0, ani )
plt.close()
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.427248545333861
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.4101714270327574
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3937409639386623
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3779376256918557
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.36274289451176367
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.34813872614862
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3341071917948226
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3206302826605762
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.3076898436917607
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2952675979268856
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.283345225697623
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2719044698194792
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.260927246136014
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2503957463555324
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.2402925261448335
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.23060057572643106
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.22130337296413982
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.21238492046991433
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.20382976898324276
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.19562302946505558
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18775037623520272
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.18019804321845673
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.17295281504459958
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.16600201442878232
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.15933348696755512
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1529355842356561
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14679714586108872
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.14090748108845594
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.13525635020795138
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1298339461241817
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.12463087625965699
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11963814492752411
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11484713626282267
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.11024959776786698
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.1058376245026108
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.10160364393291016
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09754040143682305
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.09364094646019322
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08989861930679952
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08630703854457221
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.08286008900726023
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07955191037004106
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07637688627766671
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07332963400459311
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.07040499462808845
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06759802369747066
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06490398238547578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.06231832911138062
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.059836711630158315
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.057454959587968146
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.055169077552280454
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.05297523853576866
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.050869778048177716
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04884918873185757
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.046910115668050915
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.04504935248806578
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.043263838495802794
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.041550657122410364
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.039907036219881134
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03833035101318468
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03681813107603778
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03536807368735076
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.03397806781863398
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.032646236828468005
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0313710162194182
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.0301513022994921
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.028986758575354973
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.027878519877879235
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.026831090769848288
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
err 0.02585892050707842

Cheating with Jacobians

Since the Jacobian already estimates how much the target position will change, we can cheat a little bit with respect to calculating the pseudo inverse of the Jacobian

We can use the transpose of the Jacobian directly.

Much simpler and faster to compute that calculating the pseudo inverse. No need to invert the matrix

current_angles = np.array( solution1 )
current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])

target_angles = np.array( solution2 )
target_position =  thormang_forward_kinematics( target_angles ).dot([0,0,0.1,1])

d_err = (target - current_position)[0:3]

alpha = 0.5

count = 0
errs = []

while( np.linalg.norm(d_err) > 0.1 ):
  current_position = thormang_forward_kinematics( current_angles ).dot([0,0,0.1,1])
  d_err = (target - current_position)[0:3]
  errs.append( np.linalg.norm(d_err) )

  print("d_err", np.linalg.norm(d_err), "target", target, "current_position", current_position )
  jac = Jacobian(current_angles, fwd_kinematics)

  d_theta = jac.T.dot(d_err) 

  current_angles = current_angles + alpha * d_theta

  count = count + 1
  if (count > 40 ):
    break

fig = plt.figure()
ax = fig.add_subplot(1,1,1)

ax.plot(errs, "b-", linewidth=2)

e2 = addJBFigure("e2", 0, 0, fig )
plt.close()
d_err 0.6803841190440875 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.14268179 0.39627408 0.64713211 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.6556877831256608 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.14272912 0.43168698 0.64124858 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.6275884590936451 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.14076922 0.46957246 0.63215939 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.5958663769474063 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.13620501 0.50921155 0.61922448 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.5605446657146683 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.12851698 0.54951159 0.60200832 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.5219699943239683 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.11738686 0.58907181 0.58041933 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.48085065571913777 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.1028155  0.62634976 0.55480763 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.43822474048676946 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.08518878 0.6599036  0.52597257 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.39535071615695483 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.06525349 0.68864129 0.49505858 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.3535427021310278 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.04399917 0.71199147 0.46336436 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.3139963456625801 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.02248373 0.72994042 0.43212972 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.2776531669797927 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [0.00166362 0.74293878 0.40236797 1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.24513109620720508 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.01772182  0.75173196  0.3747824   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.21672111533963306 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.03519396  0.75718159  0.3497645   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.19243055493165862 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.05051863  0.7601259   0.32744603  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.1720488056850544 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.06365588  0.76129577  0.30777203  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.15521683074486808 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.07470011  0.76128051  0.29057099  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.14149033740115458 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.08382601  0.76052745  0.27561074  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.13039263807323392 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.09124682  0.75935963  0.2626375   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.12145596831712868 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.09718562  0.75799998  0.2514004   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.11425081147626118 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.10185759  0.75659545  0.24166512  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.10840340461071328 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.10546023  0.7552378   0.2332204   1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.10360271280710157 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.10816907  0.75398016  0.22588007  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics
d_err 0.09959929732136613 target [-0.06981941  0.79628005  0.1394304   1.        ] current_position [-0.11013661  0.75284956  0.21948249  1.        ]
fwd_kinematics
fwd_kinematics
fwd_kinematics
fwd_kinematics